% calculate the outputs for AMBER model
% outputs include: 1. hip position
%                  2. linearized hip position
%                  3. non-stance slope
%                  4. linearized non-stance slope
%                  5. stance COM slope
%                  6. linearized stance COM slope
function outputs = cal_robot_para_18(q,model_name)

if strcmp(model_name,'AMBER')
addpath('./models/AMBER/build_torso');
elseif strcmp(model_name,'NAO')
    addpath('./models/NAO/build_torso');
elseif strcmp(model_name,'HM')
    addpath('./models/human/build_torso')
end

for i=1:size(q,2)
    % 1. hip position
    hippos(i) = hipposFn(q(:,i));
    % 2. linearized hip position
    delta_hippos(i) = deltaHipposFn(q(:,i));
    % 3. nonstance slope
    nsslope(i) = nsslopeFn(q(:,i));
    % 4. linearized non-stance slope
    delta_nsslope(i) = deltaNSslopeFn(q(:,i));
    % torso related angle
    % 5. stance COM slope
    stCOM(i) = stCOMFn(q(:,i));
    % 6. linearized stance COM slope
    delta_stCOM(i) = deltastCOMFn(q(:,i));
    % 7. non-stance COM slope
    nstCOM(i) = nstCOMFn(q(:,i));
    % 8. linearized non-stance COM slope
    delta_nstCOM(i) = deltanstCOMFn(q(:,i));
end
outputs.hippos = hippos-hippos(1);
outputs.delta_hippos = delta_hippos-delta_hippos(1);
outputs.nsslope = nsslope;
outputs.delta_nsslope = delta_nsslope;
outputs.stCOM = stCOM;
outputs.delta_stCOM = delta_stCOM;
outputs.nstCOM = nstCOM;
% plot(nstCOM); hold on;
outputs.delta_nstCOM = delta_nstCOM;
end